%% 指明设备类型
clear;close all;
% device = 'zzuNeural'; 
device = '3birdPosAtt'; 

%% 获得飞行阶段的imu和gps数据
% 1.运行该节程序，检查py合并后的mpu6050原始数据,然后根据这个数据确定一下gps和imu时间戳开始的位置，然后删除imu和gps前面没用的数据
% 2.绘制gps路径，然后根据行号删除gps文件中快到家的模棱两可的数据，这样可以根据gps获得飞行阶段的结束的时间戳，即能获得extract_gps_data.txt
% 3.根据gps结束的时间戳，截取已经删除前面没用数据的imu文件的飞行段数据，即获得extract_imu_data.txt，再次运行该程序使得工作区加载飞行段imu的数据，同时检查数据合格否
% 零偏需要提前测试，不同设备的不一样

% zzuNeural device_1的零偏
% acc_bias = [0.160780855315328,-0.140685535841661,0.435877629521006];
% gyro_bias = [3.223180831679578,-0.243813734339135,0.766613081504240];

% 3birdPosAtt device_1的零偏
acc_bias = [-0.686192379006895,-0.959031736654023,0.278989991367265]; 
gyro_bias = [-1.195969455791829,-3.213544079787970,-31.651342950795320];

% 3birdPosAtt device_2的零偏
% acc_bias = [0.100351585632734,1.795577932774103,2.406156748488472]; 
% gyro_bias = [-19.101083023245476,5.233274521758554,31.216475753624640];

% imudata = load("D:\About_code\FlyNueralProject\original_data\P35\20240908\Attitude data\progressed_data\imu_data.txt");
imudata = load("C:\Users\HP\Desktop\3birds\1.ALLTEST\20240910_IntegratedNavigation\device_1\progressed_data\imu_data.txt");
if strcmp(device, 'zzuNeural')
    % 不带欧拉角的数据
    AccXYZ = (imudata(:,1:3)-32768) * 16 * 9.7966 / 32768;% m^2/s, Range：±4g
    AngVelXYZ = (imudata(:,4:6)-32768) * 1000 / 32768;% °/s, Range：±500
    AccXYZ = AccXYZ + acc_bias;
    AngVelXYZ = AngVelXYZ + gyro_bias;
    AtimeStamp = imudata(:,7);
    
    figure;
    plot(AtimeStamp, AccXYZ(:,1), 'r', AtimeStamp, AccXYZ(:,2), 'g', AtimeStamp, AccXYZ(:,3), 'b');
    xlabel('TimeStamp(ms)');
    ylabel('Acc(m/s^2)');
    title('Acc-TimeStamp');
    legend('AccX', 'AccY', 'AccZ');  % 添加图例
    figure;
    plot(AtimeStamp, AngVelXYZ(:,1), 'r', AtimeStamp, AngVelXYZ(:,2), 'g', AtimeStamp, AngVelXYZ(:,3), 'b');
    xlabel('TimeStamp(ms)');
    ylabel('Gyro(°/s)');
    title('Gyro-TimeStamp');
    legend('GyroX', 'GyroY', 'GyroZ');  % 添加图例

    % 带欧拉角的数据
%     Attitude = (imudata(:,1:3)-1800)/10; % Yaw, Picth, Roll, °
%     AccXYZ = (imudata(:,4:6)-32768) * 8 * 9.7966 / 32768;% m^2/s, Range：±4g
%     AngVelXYZ = (imudata(:,7:9)-32768) * 1000 / 32768;% °/s, Range：±500
%     AtimeStamp = imudata(:,10);
elseif strcmp(device, '3birdPosAtt')
    Attitude = imudata(:,1:3); % q0, q1, q2, q3
    AccXYZ = (imudata(:,5:7)-32768) * 16 * 9.7966 / 32768;% m^2/s, Range：±16g
    AngVelXYZ = (imudata(:,8:10)-32768) * 2000 / 32768;% °/s, Range：±1000
    AccXYZ = AccXYZ + acc_bias;
    AngVelXYZ = AngVelXYZ + gyro_bias;
    Height = imudata(:,12)/100;
    AtimeStamp = imudata(:,13);
    
    figure;
    plot(AtimeStamp, AccXYZ(:,1), 'r', AtimeStamp, AccXYZ(:,2), 'g', AtimeStamp, AccXYZ(:,3), 'b');
    xlabel('Index(Interval about 5ms)');
    ylabel('Acc(m/s^2)');
    title('Acc-Index');
    legend('AccX', 'AccY', 'AccZ');  % 添加图例
    figure;
    plot(AtimeStamp, AngVelXYZ(:,1), 'r', AtimeStamp, AngVelXYZ(:,2), 'g', AtimeStamp, AngVelXYZ(:,3), 'b');
    xlabel('Index(Interval about 5ms)');
    ylabel('Gyro(°/s)');
    title('Gyro-Index');
    legend('GyroX', 'GyroY', 'GyroZ');  % 添加图例
    figure;
    plot(AtimeStamp, Height, 'r');
    xlabel('Index(Interval about 5ms)');
    ylabel('Altitude(m)');
    title('Altitude-Index');
    legend('Altitude');  % 添加图例
end  


%% 保存为imu_data.mat
if strcmp(device, 'zzuNeural')
    %调整飞行段imudata的时间戳，并最终用于组合导航和提取对应神经信号的imu_data.mat
    imu_data_inequalinterval = imudata;
    imu_data_inequalinterval(:,1:3) = AccXYZ;
    imu_data_inequalinterval(:,4:6) = AngVelXYZ;
    % 将imudata的时间戳严格等于5
    timestamps = imu_data_inequalinterval(:,7);
    new_timestamps = (min(timestamps):5:max(timestamps))';% 新时间戳间隔为5的时间戳
    imu_data = zeros(size(new_timestamps,1),7);
    for i=1:6
        imu_data(:,i) = interp1(timestamps, imu_data_inequalinterval(:,i), new_timestamps, 'linear');% 使用线性插值对原数据进行修正
    end
    imu_data(:,7) = new_timestamps;
    save("D:\About_code\FlyNueralProject\dataset\P35\20240907\imu_data.mat",'imu_data');
    
 elseif strcmp(device, '3birdPosAtt')
    imu_data(:,1:3) = AccXYZ;
    imu_data(:,4:6) = AngVelXYZ;
    imu_data(:,7) = Height;
    imu_data(:,8) = AtimeStamp;

    save("C:\Users\HP\Desktop\3birds\1.ALLTEST\20240910_IntegratedNavigation\device_1\progressed_data\imu_data.mat",'imu_data');
end  

%% 这步再去组合导航验证一下imu_data.mat和gps_data.txt是否可用

%% 神经信号提取
% 神经信号开始行索引 = imu_data开始时间戳 - start_device_timestamp + 1
% 神经信号结束行索引 = imu_data行数*5 + 神经信号开始行索引 - 1

clear; %清空工作区中的所有变量。
clc; %清除命令行窗口
close all;  %关闭所有图形界面
start_device_timestamp = load("D:\About_code\FlyNueralProject\original_data\P35\20240907\Attitude data\progressed_data\merger_imu.txt");
start_device_timestamp = start_device_timestamp(1,end);
load("D:\About_code\FlyNueralProject\dataset\P35\20240907\imu_data.mat");%加载imu_data数据开始提取飞行段神经信号

% 加载神经信号数据，通过imu_data.txt的索引，提取飞行段神经信号
mag = 12; % 放大倍数
folder_name = uigetdir('D:\About_code\FlyNueralProject\original_data\P35\20240907', '选择文件夹'); % 让用户选择文件夹D:\About_code\FlyNueralProject\original_data\P35\20240823
if isequal(folder_name, 0)
    disp('用户取消了操作');
else
    disp(['选择的文件夹是：', folder_name]);
    [selected_files, selected_folder] = uigetfile(fullfile(folder_name, '*.*'), '选择文件', 'MultiSelect', 'on');% 使用 uigetfile 函数选择文件
    if isequal(selected_files, 0) % 检查用户是否取消了操作
        disp('用户取消了操作');
    else
        disp('选择的文件列表：'); % 显示选择的文件列表
        disp(selected_files);
        if iscell(selected_files)% 如果用户选择了多个文件，selected_files 将会是一个 cell 数组， 否则它将会是一个字符串
            for i = 1:length(selected_files)
                disp(fullfile(selected_folder, selected_files{i}));% 遍历 cell 数组，显示每个文件的完整路径
            end
        else
            disp(fullfile(selected_folder, selected_files));% 如果只选择了一个文件，直接显示其完整路径
        end
    end
end
numFiles = numel(selected_files);% 合并文件
original_neural = cell(1, numFiles);% 初始化保存输出的 cell 数组
for i = 1:numFiles  % 循环读取并合并文件内容
    file_path = fullfile(selected_folder, selected_files{i});
    
    fileID=fopen(file_path);
    D=fread(fileID,inf,'ubit24','b');  %ubitX：X位精度
    P=fix(length(D)/8);
    temp=reshape(D(1:8*P),[8,P]);
    z_data=temp';
    %转换为实际电压
    for col=1:8 %修改
        for j=1:P;
            if z_data(j,col)>=8388608
                data(j,col)=(16777216-z_data(j,col))*(-4500)/8388607;
            else
                data(j,col)=z_data(j,col)*4500/8388607;
            end
        end
    end
    original_neural{i} = data/mag;   
end
original_neural = cat(1, original_neural{:});% 拼接输出

%% 绘图检查神经信号坏道
% 神经信号前面的数先置零，然后删除后面的数，中间的数重新构建一个变量后再重命名为neural_data
% 然后绘图检查neural_data是不是存在坏道，并记录到excel中，如果有坏道，则删除后在保存neural_data.mat
figure; % 创建一个新的图形窗口
plot(neural_data);
legend('CH-1', 'CH-2', 'CH-3', 'CH-4', 'CH-5', 'CH-6', 'CH-7', 'CH-8');

%% 保存工作区内没有问题的神经信号数据
save('D:\About_code\FlyNueralProject\dataset\P35\20240907\neural_data.mat','neural_data');

%% 接下来，将imu_data.mat,gps_data.txt和neural_data.mat保存到dataset文件夹供后续分析使用，首先就应该进行神经信号的去噪

